#pragma once

namespace Tmpl {

// ===================================================================================
// 2D Vector class
// ===================================================================================

template <typename T> class Vec2
{
public:
	Vec2() : x(0), y(0)							{}
	Vec2(T X, T Y) : x(X), y(Y)					{}

	void Set(T X, T Y)							{x = X, y = Y;}
	void Normalize()							{T r = 1.0f/Length(); x*=r, y*=r;}
	void Truncate(T max)						{if(x*x+y*y > max*max){T r = max/Length(); x*=r, y*=r;}}
	void Invert()								{x = -x, y = -y;}

	T Length() const;
	T SqrLength() const							{return (x*x + y*y);}
	T Dot(const Vec2& v) const					{return (x*v.x + y*v.y);}
	Vec2 Reflected(const Vec2& n) const			{return ((*this) - (2 * Dot(n)) * n);}
	Vec2 Normalized() const						{const T r = 1/Length(); return Vec2(x*r, y*r);}
	Vec2 Perp() const							{return Vec2(y, -x);}
	int Sign(const Vec2& n)const				{return (y*n.x > x*n.y) ? -1 : 1; }

	T& operator[] (int index)					{return cell[index];}
	const T& operator[] (int index) const		{return cell[index];}
	operator T*()								{return cell;}
	operator const T*() const					{return cell;}
	
	Vec2 operator-() const						{return Vec2(-x, -y);}
	void operator += (const Vec2& v)			{x += v.x, y += v.y;}
	void operator -= (const Vec2& v)			{x -= v.x, y -= v.y;}
	void operator *= (const Vec2& v)			{x *= v.x, y *= v.y;}
	void operator *= (T f)						{x *= f, y *= f;}
	void operator /= (T f)						{T r = 1/f; x *= r, y *= r;}

	bool operator == (const Vec2& v) const		{return (x == v.x && y == v.y);}
	bool operator != (const Vec2& v) const		{return (x != v.x || y != v.y);}

	friend Vec2 operator + (const Vec2& v1, const Vec2& v2)	{return Vec2(v1.x+v2.x, v1.y+v2.y);}
	friend Vec2 operator - (const Vec2& v1, const Vec2& v2)	{return Vec2(v1.x-v2.x, v1.y-v2.y);}
	friend Vec2 operator * (const Vec2& v1, const Vec2& v2)	{return Vec2(v1.x*v2.x, v1.y*v2.y);}
	friend Vec2 operator * (T f, const Vec2& v)				{return Vec2(v.x*f, v.y*f);}
	friend Vec2 operator * (const Vec2& v, T f)				{return Vec2(v.x*f, v.y*f);}
	friend Vec2 operator / (const Vec2& v, T f)				{T r = 1/f; return Vec2(v.x*r, v.y*r);}

	union
	{
		struct {T x, y;};
		struct {T cell[2];};
	};
};

typedef Vec2<float> Vec2f;
typedef Vec2<int>	Vec2i;

template <typename T> class Vec2s
{
public:
	Vec2s<T>& operator = (const Vec2<T> &rhs) { return *this; }
	operator Vec2<T>(){return Vec3<T>(x,y);}
	struct {T x, y;};
};
typedef Vec2s<float> Vec2sf;
typedef Vec2s<int>	Vec2si;

// ===================================================================================
// 3D Vector class
// ===================================================================================

template <typename T> class Vec3
{
public:
	Vec3() : x(0), y(0), z(0)					{}
	Vec3(T X, T Y, T Z) : x(X), y(Y), z(Z)		{}

	void Set(T X, T Y, T Z)						{x = X, y = Y, z = Z;}
	void Normalize()							{T r = 1.0f/Length(); x *= r, y *= r, z *= r;}
	void Truncate(T max)						{if(x*x+y*y+z*z > max*max){T r=max/Length(); x*=r,y*=r,z*=r;}}
	void Invert()								{x = -x, y = -y, z = -z;}

	T Length() const;
	T SqrLength() const							{return (x*x + y*y + z*z);}
	T Dot(const Vec3& v) const					{return (x*v.x + y*v.y + z*v.z);}
	Vec3 Cross(const Vec3& v) const				{return Vec3(y*v.z - z*v.y, z*v.x - x*v.z, x*v.y - y*v.x);}
	Vec3 Reflected(const Vec3& n) const			{return ((*this) - (2 * n.Dot(*this)) * n);}
	Vec3 Normalized() const						{T r = 1/Length(); return Vec3(x*r, y*r, z*r);}

	T& operator[] (int index)					{return cell[index];}
	const T& operator[] (int index) const		{return cell[index];}
	operator T*()								{return cell;}
	operator const T*() const					{return cell;}
	
	Vec3 operator-() const						{return Vec3(-x, -y, -z);}
	void operator += (const Vec3& v)			{x += v.x, y += v.y, z += v.z;}
	void operator -= (const Vec3& v)			{x -= v.x, y -= v.y, z -= v.z;}
	void operator *= (const Vec3& v)			{x *= v.x, y *= v.y, z *= v.z;}
	void operator *= (const T f)				{x *= f, y *= f, z *= f;}
	void operator /= (T f)						{T r = 1/f; x *= r, y *= r, z *= r;}

	bool operator == (const Vec3& v) const		{return (x == v.x && y == v.y && z == v.z);}
	bool operator != (const Vec3& v) const		{return (x != v.x || y != v.y || z != v.z);}

	friend Vec3 operator + (const Vec3& v1, const Vec3& v2)	{return Vec3(v1.x+v2.x, v1.y+v2.y, v1.z+v2.z);}
	friend Vec3 operator - (const Vec3& v1, const Vec3& v2)	{return Vec3(v1.x-v2.x, v1.y-v2.y, v1.z-v2.z);}
	friend Vec3 operator * (const Vec3& v1, const Vec3& v2)	{return Vec3(v1.x*v2.x, v1.y*v2.y, v1.z*v2.z);}
	friend Vec3 operator * (T f, const Vec3& v)				{return Vec3(v.x*f, v.y*f, v.z*f);}
	friend Vec3 operator * (const Vec3& v, T f)				{return Vec3(v.x*f, v.y*f, v.z*f);}
	friend Vec3 operator / (const Vec3& v, T f)				{T r = 1.0f/f; return Vec3(v.x*r, v.y*r, v.z*r);}
	
	union
	{
		struct {T x, y, z;};
		struct {T cell[3];};
	};
};

typedef Vec3<float> Vec3f;
typedef Vec3<int>	Vec3i;

template <typename T> class Vec3s
{
public:
	Vec3s<T>& operator = (const Vec3<T> &rhs) { return *this; }
	operator Vec3<T>(){return Vec3<T>(x,y,z);}
	struct {T x, y, z;};
};
typedef Vec3s<float> Vec3sf;
typedef Vec3s<int>	Vec3si;

// ===================================================================================
// 3x3 Tranformation matrix class (2D)
// ===================================================================================

template <typename T> class Matrix3x3
{
public:
	enum{ TX=6, TY=7, D0=0, D1=4, D2=8, SX=D0, SY=D1, SZ=D2 };
	
	Matrix3x3()								{Identity();}
	Matrix3x3(const Vec2<T>& pos, T rot)	{Init(pos, rot);}
	operator const T*() const				{return cell;}
	operator T*()							{return cell;}

	void Identity()
	{
		for(int i=0; i<9; ++i) cell[i] = 0.0f;
		cell[D0] = cell[D1] = cell[D2] = 1.0f;
	}
	void Init(const Vec2<T>& pos, T rot)
	{
		Rotate(rot);
		Translate(pos);
	}
	void Rotate(const Vec2<T>& forward, const Vec2<T>& side)
	{
		Identity();
		cell[0] = forward.x;	cell[1] = forward.y;
		cell[3] = side.x;		cell[4] = side.y;
	}
	void Rotate(T rot);

	void Translate(const Vec2<T>& pos)		{cell[TX] += pos.x, cell[TY] += pos.y;}
	void Translate(T x, T y, T z)			{cell[TX] += x, cell[TY] += y;}
	void SetTranslation(const Vec2<T>& pos)	{cell[TX] = pos.x, cell[TY] = pos.y;}
	void SetTranslation(T x, T y, T z)		{cell[TX] = x, cell[TY] = y;}
	Vec2<T> GetTranslation() const			{return Vec2<T>(cell[TX], cell[TY]);}

	Vec2<T> Transform(const Vec2<T>& v) const
	{
		T x = cell[0] * v.x + cell[3] * v.y + cell[TX];
		T y = cell[1] * v.x + cell[4] * v.y + cell[TY];
		return Vec2<T>(x, y);
	}
	void Concatenate(const Matrix3x3& m2)
	{
		Matrix3x3 res;
		for(int y=0; y<3; ++y)
		{
			for(int x=0; x<3; ++x)
			{
				res.cell[y*3 + x] =	cell[x + 0] * m2[y*3 + 0] +
									cell[x + 3] * m2[y*3 + 1] +
									cell[x + 6] * m2[y*3 + 2];
			}
		}
		for(int i=0; i<9; ++i) cell[i] = res.cell[i];
	}
	void Transpose()
	{
		Matrix3x3 r;
		for(int h=0; h<3; h++) for (int v=0; v<3; v++) r.cell[h + v*3] = cell[v + h*3];
		for(int i=0; i<9; ++i) cell[i] = r.cell[i];
	}
	friend Vec2<T> operator * (const Matrix3x3& m, const Vec2<T>& v)
	{ 
		return m.Transform(v);
	}
	friend Matrix3x3 operator * (const Matrix3x3& m, const Matrix3x3& n)
	{
		Matrix3x3 tmp = m;
		tmp.Concatenate(n);
		return tmp;
	}
private:
	T cell[16];
};

typedef Matrix3x3<float> Matrix3x3f;

// ===================================================================================
// 4x4 Tranformation matrix class (3D)
// ===================================================================================

template <typename T> class Matrix4x4
{
public:
	enum{ TX=12, TY=13, TZ=14, D0=0, D1=5, D2=10, D3=15, SX=D0, SY=D1, SZ=D2, W=D3 };
	
	Matrix4x4()													{Identity();}
	Matrix4x4(const Vec3<T>& pos, T rotX, T rotY, T rotZ)		{Init(pos, rotX, rotY, rotZ);}
	operator const T*() const									{return cell;}
	operator T*()												{return cell;}

	void Identity()
	{
		for(int i=0; i<16; ++i) cell[i] = 0.0f;
		cell[D0] = cell[D1] = cell[D2] = cell[D3] = 1.0f;
	}
	void Init(const Vec3<T>& pos, T rotX, T rotY, T rotZ)
	{
		SetRotation(rotX, rotY, rotZ);
		Translate(pos);
	}
	void RotateX(T rotX);
	void RotateY(T rotY);
	void RotateZ(T rotZ);

	void SetRotateX(T rotX);
	void SetRotateY(T rotY);
	void SetRotateZ(T rotZ);

	void SetPerspective(T fov, T aspect, T zNear, T zFar);

	void SetRotation(const Vec3<T>& rot)
	{
		SetRotation(rot.x, rot.y, rot.z);
	}
	void SetRotation(T x, T y, T z)
	{
		Matrix4x4 m; Identity();
		m.SetRotateX(x); Concatenate(m);
		m.SetRotateY(y); Concatenate(m);
		m.SetRotateZ(z); Concatenate(m);
	}
	void LookAt(const Vec3<T>& direction, const Vec3<T>& upVec)
	{
		const Vec3<T> Z = -direction.Normalized();
		const Vec3<T> X = Z.Cross(upVec).Normalized();
		const Vec3<T> Y = Z.Cross(X).Normalized();
		Identity();
		for(int i=0; i<3; ++i)
		{
			cell[0+i*4] = X[i];
			cell[1+i*4] = Y[i];
			cell[2+i*4] = Z[i];
		}
	}
	void Translate(const Vec3<T>& pos)		{cell[TX] += pos.x, cell[TY] += pos.y, cell[TZ] += pos.z;}
	void Translate(T x, T y, T z)			{cell[TX] += x, cell[TY] += y, cell[TZ] += z;}
	void SetTranslation(const Vec3<T>& pos)	{cell[TX] = pos.x, cell[TY] = pos.y, cell[TZ] = pos.z;}
	void SetTranslation(T x, T y, T z)		{cell[TX] = x, cell[TY] = y, cell[TZ] = z;}
	Vec3<T> GetTranslation() const			{return Vec3<T>(cell[TX], cell[TY], cell[TZ]);}

	Vec3<T> Transform(const Vec3<T>& v) const
	{
		T x = cell[0] * v.x + cell[4] * v.y + cell[8] * v.z + cell[TX];
		T y = cell[1] * v.x + cell[5] * v.y + cell[9] * v.z + cell[TY];
		T z = cell[2] * v.x + cell[6] * v.y + cell[10]* v.z + cell[TZ];
		return Vec3<T>( x, y, z );
	}
	void Concatenate(const Matrix4x4& m2)
	{
		Matrix4x4 res;
		for(int y=0; y<4; ++y)
		{
			for(int x=0; x<4; ++x)
			{
				res.cell[y*4 + x] =	cell[x + 0 ] * m2[y*4 + 0] +
									cell[x + 4 ] * m2[y*4 + 1] +
									cell[x + 8 ] * m2[y*4 + 2] +
									cell[x + 12] * m2[y*4 + 3];
			}
		}
		for(int i=0; i<16; ++i) cell[i] = res.cell[i];
	}
	void Transpose()
	{
		Matrix4x4 r;
		for(int h=0; h<4; h++) for (int v=0; v<4; v++) r.cell[h + v*4] = cell[v + h*4];
		for(int i=0; i<16; ++i) cell[i] = r.cell[i];
	}
	void Invert()
	{
		Matrix4x4 r;
		T tx = -cell[TX], ty = -cell[TY], tz = -cell[TZ];
		for(int h = 0; h < 3; h++) for (int v = 0; v < 3; v++) r.cell[h + v*4] = cell[v + h*4];
		for(int i = 0; i < 16; i++) cell[i] = r.cell[i];
		cell[TX] = tx * cell[0] + ty * cell[4] + tz * cell[8];
		cell[TY] = tx * cell[1] + ty * cell[5] + tz * cell[9];
 		cell[TZ] = tx * cell[2] + ty * cell[6] + tz * cell[10];
	}
	friend Vec3<T> operator * (const Matrix4x4& m, const Vec3<T>& v)
	{ 
		return m.Transform(v);
	}
	friend Matrix4x4 operator * (const Matrix4x4& m, const Matrix4x4& n)
	{
		Matrix4x4 tmp = m;
		tmp.Concatenate(n);
		return tmp;
	}
private:
	T cell[16];
};

typedef Matrix4x4<float> Matrix4x4f;

// ===================================================================================
// Basic Helper Functions
// ===================================================================================
template <typename T> 
Vec2<T> PointToWorldSpace(const Vec2<T>& point, const Vec2<T>& forward, const Vec2<T>& side, const Vec2<T>& pos)
{
	Matrix3x3<T> m;
	m.Rotate(forward, side);
	m.SetTranslation(pos);
	return m.Transform(point);
}

template <typename T> 
Vec2<T> VectorToWorldSpace(const Vec2<T>& vec, const Vec2<T>& forward, const Vec2<T>& side)
{
	Matrix3x3<T> m;
	m.Rotate(forward, side);
	return m.Transform(vec);
}

template <typename T> 
Vec2<T> PointToLocalSpace(const Vec2<T>& point, const Vec2<T>& forward, const Vec2<T>& side, const Vec2<T>& pos)
{
	Matrix3x3<T> m;
	m.Rotate(Vec2<T>(forward.x, side.x), Vec2<T>(forward.y, side.y));
	m.SetTranslation(Vec2<T>(-pos.Dot(forward), -pos.Dot(side)));
	return m.Transform(point);
}

template <typename T> 
Vec2<T> VectorToLocalSpace(const Vec2<T>& vec, const Vec2<T>& forward, const Vec2<T>& side)
{
	Matrix3x3<T> m;
	m.Rotate(Vec2<T>(forward.x, side.x), Vec2<T>(forward.y, side.y));
	return m.Transform(vec);
}

}; // namespace Tmpl